#include "Mytask.h"

double home_angles[6]    ={0,85.0,86.0,78.0,85.0,100};//标定初始竖直位姿
double now_temp_angles[6]={0,85.0,86.0,78.0,85.0,100};//打角期间临时位姿
double last_angles[6]    ={0,85.0,86.0,78.0,85.0,100};//上一次位姿
double theta_angles[6]={0,0,0,0,0,0};
double idle_angles[6]    ={0,140.0,180.0,45.0,85.0,60.0};
double receive_angles[6]={0,0,0,0,0,0};//接收上位机的关节角度
double limit_angles[5]={0,0,0,0,0};
double steer_out[5]={0,0,0,0,0};
uint8_t buffer[sizeof(home_angles)+1];
uint8_t rx_angles_flag = 0;
uint16_t adc2_value = 0;

extern RobotAxis axis;
extern UART_HandleTypeDef huart1;

JointAngles joint_angles;
KinematicsState  Kinematics_state = wait;
ServoState  Servo_state = servo_wait;
SerialState Serial_state = uart_wait;

RobotAxis Button_select_state = AXIS_ALL_ARMS;
int8_t state_cnt = -1;
uint8_t Button_control_flag = 0;
uint8_t adc_auto_refresh = 0;   // 0=停止  1=运行
// 定义接收缓冲区
uint8_t RxPacket[49];  // 1 (header) + 6 * 8 (floats) = 49 bytes

PoseData Pose;
AnglesData Angles;

ArmDimensions dim = {
		.l1 = 0,  
		.l2 = 80.0f,
		.l3 = 100.0f,
		.l4 = 50.0f,
		.l5 = 90.0f
};

EndEffectorPose grip_pose = {
    .x = 188.516464f,
    .y = 188.516464f,
    .z = 19.282032f,
    .pitch = 0,  
    .roll = 0 
};
EndEffectorPose release_pose = {
    .x = -188.516464f,
    .y = 188.516464f,
    .z = 119.282032f,
    .pitch = 0,  
    .roll = 0   
};
EndEffectorPose idle_pose = {
    .x = 108.994949f,
    .y = 108.994949f,
    .z = 127.279221f,
    .pitch = 0,  
    .roll = 0   
};

/**
 * @brief 串口1接收位姿变化中断回调函数
 * @return 无
 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) 
{
    osEventFlagsSet(evt_id_LED, 0x02U);   // bit1 → Blue 控制LED亮工作灯
    Serial_state=uart_wait;
    uint8_t header = RxPacket[0];
    if (huart->Instance == USART1 ) 
    {
        switch (header)
        {
        case 0xA5:
            memcpy(&Pose, RxPacket, sizeof(PoseData));
            grip_pose.x = Pose.grip_x;
            grip_pose.y = Pose.grip_y;
            grip_pose.z = Pose.grip_z;
            release_pose.x = Pose.release_x;
            release_pose.y = Pose.release_y;
            release_pose.z = Pose.release_z;
            Serial_state = uart_ready;
            Kinematics_state = grip;
            break;
        
        case 0x5A:
            memcpy(&Angles, RxPacket, sizeof(AnglesData));
            receive_angles[0] = Angles.axis_1;
            receive_angles[1] = Angles.axis_2;
            receive_angles[2] = Angles.axis_3;
            receive_angles[3] = Angles.axis_4;
            receive_angles[4] = Angles.axis_5;
            receive_angles[5] = Angles.axis_6;

            Serial_state = uart_ready;
            rx_angles_flag = 1;
            Servo_state = servo_control;
            break;

        }
        
    }
    HAL_UART_Receive_IT(huart, RxPacket, sizeof(RxPacket));
}

void Send_update_angles(void)
{
    buffer[0] = 0x5A;

    memcpy(buffer+1, now_temp_angles, sizeof(now_temp_angles));

    HAL_UART_Transmit(&huart1, buffer, sizeof(buffer), 100);
}

void Servo_Control_task(void)
{
    // static uint8_t Kinematics_flag = 0;
    
    // if(Kinematics_state==grip && Serial_state==uart_ready) {
    //     Kinematics_flag = inverseKinematics(&dim ,&grip_pose ,&joint_angles);//计算基于标定姿态到目标位姿的角度变化值并且存到joint_angles
    //     Kinematics_state = waiting_g;
    // }
    // else if (Kinematics_state==release && Serial_state==uart_ready) {
    //     Kinematics_flag = inverseKinematics(&dim ,&release_pose ,&joint_angles);//计算基于标定姿态到目标位姿的角度变化值并且存到joint_angles
    //     Kinematics_state = waiting_r;
    // }
    
    // if(Kinematics_flag!=0) 
    // {
    //     Axis_remaining_bais(&joint_angles,last_angles,theta_angles);//计算剩余的打角误差

    //     for(uint8_t i=0;i<Axis_num;i++)
    //     {
    //         steer_out[i] = theta_angles[i];
    //     }

    //     Axis_Speed_Limit(steer_out,limit_angles, 6);//限制舵机角度值的变化
    //     Axis_now_angle_updata(now_temp_angles,last_angles,limit_angles);//更新当前角度值

    //     if(Axis_angle_limit(now_temp_angles)!=0) 
    //     {
    //         Robot_jointAxis_SetAngle(now_temp_angles);
            
    //         osEventFlagsSet(evt_id_LED, 0x02U);   // bit1 → Blue 控制LED亮工作灯

    //         // 使用队列发送OLED消息
    //         oledMsg = (OLED_Message)
    //         {
    //             .line = 1,
    //             .column = 1,
    //             .text = "workingok"
    //         };
    //         xQueueSend(xOLEDQueue, &oledMsg, pdMS_TO_TICKS(100));
    //     }
    //     else 
    //     {
    //         Robot_jointAxis_SetAngle(now_temp_angles);

    //         osEventFlagsSet(evt_id_LED, 0x01U);   // bit0 → Red 控制LED亮警告灯
            
    //         // 使用队列发送OLED消息
    //         oledMsg = (OLED_Message)
    //         {
    //             .line = 1,
    //             .column = 1,
    //             .text = "steerout"
    //         };
    //         xQueueSend(xOLEDQueue, &oledMsg, pdMS_TO_TICKS(100));
    //     }

    // }
    
    // if(Position_Succes(Position_state)!=0)//判断所有关节是否到指定姿态
    // {
    //     Gripper_Close(55);
    //     // 使用队列发送OLED消息
    //     oledMsg = (OLED_Message)
    //     {
    //         .line = 1,
    //         .column = 1,
    //         .text = "gripok"
    //     };
    //     xQueueSend(xOLEDQueue, &oledMsg, pdMS_TO_TICKS(100));
    //     if(Kinematics_state == waiting_g)
    //     {
    //         Kinematics_state = release;
    //         Kinematics_flag = 0;
    //     }
    //     else if (Kinematics_state == waiting_r)
    //     {
    //         Kinematics_state = finished;
    //         Kinematics_flag = 0;

    //     }
    //     Position_Clear();//清除所有关节到达标志位
        
    // }

    static uint8_t Kinematics_flag = 0;
    uint32_t evt_flag = 0;

        switch (Servo_state)
        {
        case servo_wait:
            if(Kinematics_state==grip && Serial_state==uart_ready) {
                Kinematics_flag = inverseKinematics(&dim ,&grip_pose ,&joint_angles);//计算基于标定姿态到抓取位姿的角度变化值并且存到joint_angles
                Serial_state = uart_wait;  // 重置状态
                Servo_state = servo_control;
            }
            else if (Kinematics_state==release) {
                Kinematics_flag = inverseKinematics(&dim ,&release_pose ,&joint_angles);//计算基于标定姿态到释放位姿的角度变化值并且存到joint_angles
                Servo_state = servo_control;
            }
            else if (Kinematics_state==idle)
            {
                Kinematics_flag = inverseKinematics(&dim ,&idle_pose ,&joint_angles);//计算基于标定姿态到等待位姿的角度变化值并且存到joint_angles
                Servo_state = servo_control;
            }
            
            break;

        case servo_control:

            if(Kinematics_flag!=0 || rx_angles_flag!=0) 
            {
                if(Kinematics_flag!=0)
                {
                    Publish_system_state(3, 7, "IK SUCCESS");
                }
                else if (rx_angles_flag!=0)
                {
                    Publish_system_state(2, 6, "rxcontrol ");
                    Publish_system_state(3, 7, "RX SUCCESS");
                    
                    joint_angles.theta1 = receive_angles[0] - home_angles[0];
                    joint_angles.theta2 = receive_angles[1] - home_angles[1];
                    joint_angles.theta3 = receive_angles[2] - home_angles[2];
                    joint_angles.theta4 = receive_angles[3] - home_angles[3];
                    joint_angles.theta5 = receive_angles[4] - home_angles[4];
                }

                Axis_remaining_bais(&joint_angles,last_angles,theta_angles);//计算剩余的打角误差

                for(uint8_t i=0;i<Axis_num;i++)
                {
                    steer_out[i] = theta_angles[i];
                }

                Axis_Speed_Limit(steer_out,limit_angles, 2);//限制舵机角度值的速度变化
                Axis_now_angle_updata(now_temp_angles,last_angles,limit_angles);//更新当前角度值

                if(Axis_angle_limit(now_temp_angles)!=0) 
                {
                    Robot_jointAxis_SetAngle(now_temp_angles);
                    
                    osEventFlagsSet(evt_id_LED, 0x02U);   // bit1 → Blue 控制LED亮工作灯
                    Publish_system_state(1, 7, "working OK");
                }
                else 
                {
                    Robot_jointAxis_SetAngle(now_temp_angles);

                    osEventFlagsSet(evt_id_LED, 0x01U);   // bit0 → Red 控制LED亮警告灯
                    Publish_system_state(1, 7, "Steer OUT ");
                    
                }
                
            }
            else//逆运动学计算失败处理
            {
                Publish_system_state(1, 7, "ERROR     ");
                Publish_system_state(2, 6, "waiting...");
                Publish_system_state(3, 7, "IK Error  ");
                osEventFlagsSet(evt_id_LED, 0x01U);   // bit0 → Red 控制LED亮警告灯
                rx_angles_flag = 0;
                Kinematics_flag = 0;
                Servo_state = servo_wait;
                break;
            }
            
            if(Position_Succes(Position_state)!=0)//判断所有关节是否到指定姿态
            {
                Timer_gripper_control(3000,Kinematics_state);

                if(rx_angles_flag == 1)
                {
                    Publish_system_state(2, 6, "controlOK  ");
                    Gripper_Angle_Limit_Control(Angles.axis_6);
                    
                    osEventFlagsSet(evt_id_Serial, 0x00000001U);   // 置位 bit0
                    Kinematics_state = wait;
                    Kinematics_flag = 0;
                    rx_angles_flag = 0;
                    Position_Clear();//清除所有关节到达标志位
                    memset(theta_angles, 0, sizeof(theta_angles));  // 重置角度
                    memset(steer_out, 0, sizeof(steer_out));

                    Servo_state = servo_wait;
                    break;
                }

                Position_Clear();//清除所有关节到达标志位
                memset(theta_angles, 0, sizeof(theta_angles));  // 重置角度
                memset(steer_out, 0, sizeof(steer_out));

                Servo_state = servo_G_R;

            }
            
            break;

        case servo_G_R:
            evt_flag = osEventFlagsWait(evt_id_Servo_GR,
                                        0x00000007U,        // 等待 bit0 | bit1
                                        osFlagsWaitAny,    // 任意一个置位即可，函数返回后会自动清除标志位
                                        osWaitForever);    // 永久等待
            if (evt_flag & 0x01U)   // bit0 夹爪成功抓取--->释放
            {
                Kinematics_state = release;
                Kinematics_flag = 0;
                Servo_state = servo_wait;
            }
            else if(evt_flag & 0x02U) // bit1 夹爪成功释放--->回归空闲等待
            {
                Kinematics_state = idle;
                Kinematics_flag = 0;
                Servo_state = servo_wait;
            }
            else if(evt_flag & 0x04U) // bit2 夹爪空闲等待--->系统准备
            {
                Kinematics_state = wait;
                Kinematics_flag = 0;
                Servo_state = servo_wait;
            }

            break;

        default:
            Robot_error_Handle("servo_G_R      ");
            break;
        }
    

}

void Gripper_Control_task(void)
{
    GripperCommand cmd;
    if (xQueueReceive(xGripperQueue, &cmd, portMAX_DELAY) == pdPASS) 
    {
        vTaskDelay(pdMS_TO_TICKS(cmd.timeout));  // 延时
        
        if (cmd.state == grip) 
        {
            Gripper_Close(55);
            Publish_system_state(2, 6, "grip       ");
            osEventFlagsSet(evt_id_Servo_GR, 0x01U);
        } 
        else if (cmd.state == release) 
        {
            Gripper_Open();
            Publish_system_state(2, 6, "release    ");
            osEventFlagsSet(evt_id_Servo_GR, 0x02U);
        }
        else if (cmd.state == idle)
        {
            Gripper_Open();
            Publish_system_state(2, 6, "idle       ");
            osEventFlagsSet(evt_id_Servo_GR, 0x04U);
        }
        else
        {
            Robot_error_Handle("Gripper Control");
        }
        
    }
    osEventFlagsSet(evt_id_Serial, 0x00000001U);   // 置位 bit0 更新角度信息
}

/**
 * @brief 发布系统状态到OLED队列
 * @param line    行号（1~4）
 * @param column  列号（1~16）
 * @param text    要显示的字符串（最大16字符 + '\0'）
 * @return pdTRUE发送成功，pdFALSE失败
 */
BaseType_t Publish_system_state(uint8_t line, uint8_t column,const char *text) {
    // 参数校验
    if (text == NULL || line == 0 || line > 4 || column > 16) {
        return pdFALSE;
    }

    OLED_Message msg = {
        .line = line,
        .column = column
    };

    // 安全拷贝字符串（确保不超过缓冲区）
    strncpy(msg.text, text, sizeof(msg.text) - 1);
    msg.text[sizeof(msg.text) - 1] = '\0';  // 强制终止符

    // 发送到队列（等待最多100ms）
    return xQueueSend(xOLEDQueue, &msg, pdMS_TO_TICKS(100));
}

void OLED_Show_task(void)
{
    OLED_Message msg;
    
    OLED_ShowString(1, 1, "servo:");
    OLED_ShowString(2, 1, "pose:");
    OLED_ShowString(3, 1, "state:");
    
    // 处理OLED消息队列
    if(xQueueReceive(xOLEDQueue, &msg, pdMS_TO_TICKS(20)) == pdPASS) {
        OLED_ShowString(msg.line, msg.column, msg.text);
    }
    
    // 等待事件标志（20ms等待，有变化才处理）
    uint32_t evt_flag = osEventFlagsWait(evt_id_OLED,
                                        0x0000007FU,      // 等待bit0-bit6
                                        osFlagsNoClear,   // 任意变化
                                        pdMS_TO_TICKS(20));              // 20ms超时
    
    // 根据标志位组合显示不同内容
     // 使用查找表映射
    char* display_texts[] = {
        "five robot arms",  // 0x01
        "  robot axis1  ",  // 0x03  
        "  robot axis2  ",  // 0x07
        "  robot axis3  ",  // 0x0F
        "  robot axis4  ",  // 0x1F
        "  robot axis5  ",  // 0x3F
        "  robot axis6  ",  // 0x7F
    };
    
    uint8_t index = 0;
    switch (evt_flag) {
        case 0x01U: index = 0; break;
        case 0x03U: index = 1; break;
        case 0x07U: index = 2; break;
        case 0x0FU: index = 3; break;
        case 0x1FU: index = 4; break;
        case 0x3FU: index = 5; break;
        case 0x7FU: index = 6; break;
        case 0xFFU: index = 7; break;
        default:    index = 0; break;
    }
    
    OLED_ShowString(4, 1, display_texts[index]);

    
}

void Button_Press_task(void)
{
    static uint8_t last_select_button_state = GPIO_PIN_SET;  // 记录上次选择按钮状态
    uint8_t current_select_button_state;
    static uint8_t last_control_button_state = GPIO_PIN_SET;  // 记录上次控制按钮状态
    uint8_t current_control_button_state;
    
    // 读取当前选择按钮状态
    current_select_button_state = HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_4);
    
    // 检测选择按钮下降沿（按下）,只在“上一次是高，这次是低”的瞬间才计数
    if (last_select_button_state == GPIO_PIN_SET && current_select_button_state != GPIO_PIN_SET) 
    {
        // 防抖延时
        osDelay(50);
        if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_4) != GPIO_PIN_SET) 
        {
            Button_control_flag = 0;
            // 按钮确认按下，增加状态计数
            state_cnt = (state_cnt + 1) % 7;  // 0-6循环
            
            // 根据状态计数设置对应的标志位
            uint32_t flags_to_set = 0;
            for (int i = 0; i <= state_cnt; i++) 
            {
                flags_to_set |= (1U << i);  // 设置bit0到bit[state_cnt]
                switch (state_cnt)
                {
                case 0:Button_select_state = AXIS_ALL_ARMS; break;
                case 1:Button_select_state = AXIS_BASE_ROTATE; break;
                case 2:Button_select_state = AXIS_SHOULDER_PITCH; break;
                case 3:Button_select_state = AXIS_ELBOW_PITCH; break;
                case 4:Button_select_state = AXIS_WRIST_PITCH; break;
                case 5:Button_select_state = AXIS_WRIST_ROTATE; break;
                case 6:Button_select_state = AXIS_GRIPPER_ROTATE; break;
                default:Button_select_state = AXIS_ALL_ARMS;break;
                }
            }
            
            // 清除所有旧标志，设置新标志
            osEventFlagsClear(evt_id_OLED, 0xFFU);
            osEventFlagsSet(evt_id_OLED, flags_to_set);
            // OLED_ShowNum(1, 8, state_cnt, 1);  // 显示当前状态
        }
    }
    
    // 更新上次按钮状态
    last_select_button_state = current_select_button_state;

    // 读取当前控制按钮状态
    current_control_button_state = HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_12);

    if (last_control_button_state == GPIO_PIN_SET &&current_control_button_state != GPIO_PIN_SET)
    {
        osDelay(50);
        if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_12) != GPIO_PIN_SET) {
            adc_auto_refresh ^= 1;                 // 0↔1 翻转
            Button_control_flag = adc_auto_refresh; // 把状态同步给原来逻辑
            Publish_system_state(3, 7,adc_auto_refresh ? "ADC ctrl " : "ADC stop  ");
        }
    }

}

void ADC_Control_task(void){

    if(state_cnt>=0)
    {
        
        Publish_system_state(1, 12,"     ");
        Publish_system_state(2, 6,"           ");
        // // ADC采集逻辑...
        HAL_ADC_Start(&hadc2);
        HAL_ADC_PollForConversion(&hadc2, 10);
        adc2_value = HAL_ADC_GetValue(&hadc2);

        double adc_angle = adc2_value * 180.0 / 4095.0;

        char buf[8];
        snprintf(buf, sizeof(buf), "%.1f", adc_angle);
        Publish_system_state(1, 7, buf);
        if (Button_control_flag && Button_select_state != AXIS_ALL_ARMS)
        {
            Servo_SetAngle(Button_select_state,(uint16_t)adc_angle);
        }
    }

    osDelay(10);  // 适当延时，避免过于频繁检测
}

void Serial_Send_task(void)
{
    /* 永久等待 bit0 被置位，等待后自动清除 */
    osEventFlagsWait(evt_id_Serial, 0x00000001U, osFlagsWaitAny, osWaitForever);
    Send_update_angles();
}

void LED_Control_task(void)
{
    uint32_t evt_flag = osEventFlagsWait(evt_id_LED,
                                        0x00000003U,      // 等待 bit0 | bit1
                                        osFlagsWaitAny,   // 任意一个置位即可
                                        osWaitForever);

    /* 2. 根据通知值执行动作 */
    if (evt_flag & 0x01U)   // bit0
    {
        Red_warning();
        Blue_OFF();
    }
    else if(evt_flag & 0x02U)   // bit1
    {
        Blue_working();
        Red_OFF();
    }
    else
    {
        Blue_OFF();
        Red_OFF();
    }
}

void Robot_error_Handle(const char* step_name){

    char name[16];
    // 安全拷贝字符串（确保不超过缓冲区）
    strncpy(name, step_name, sizeof(name) - 1);
    name[sizeof(name) - 1] = '\0';  // 强制终止符

    Publish_system_state(1, 7, "ERROR.....");
    Publish_system_state(2, 6, "ERROR.....");
    Publish_system_state(3, 7, "ERROR.....");
    Publish_system_state(4, 1, name);
    
    Red_warning();
}

